#include "App_Flight.h"
int16_t z_accel_static = 0;
Gyro_Struct gyro = {0};
// 定义两个PID结构体
PID_Struct pid_pitch = {.kp = -7, .ki = 0.0, .kd = 0.0};
PID_Struct pid_pitch_Gory = {.kp = 3, .ki = 0.0, .kd = 0.5};
// 横滚角的PID
PID_Struct pid_roll = {.kp = -7, .ki = 0.0, .kd = 0.0};
PID_Struct pid_roll_Gory = {.kp = -3, .ki = 0.0, .kd = -0.6};

// 定义两个偏航角PID结构体
PID_Struct pid_yaw = {.kp = -3.0, .ki = 0.0, .kd = 0.0};
PID_Struct pid_yaw_Gory = {.kp = -1.5, .ki = 0.0, .kd = 0.1};
// 定义一个高度PID结构体
PID_Struct pid_height = {.kp = -0.5, .ki = 0.0, .kd = 0.0};

// 定义一个高度PID结构体
PID_Struct pid_z_speed = {.kp = -2.0, .ki = 0.0, .kd = 0.0};
extern TaskHandle_t Communication_Task_Handle;
extern Motor_struct left_top_motor;
extern Motor_struct left_bottom_motor;
extern Motor_struct right_top_motor;
extern Motor_struct right_bottom_motor;
// Motor_struct left_top_motor = {.htim = &htim3, .channel = TIM_CHANNEL_1};
// Motor_struct left_bottom_motor = {.htim = &htim4, .channel = TIM_CHANNEL_4};
// Motor_struct right_top_motor = {.htim = &htim2, .channel = TIM_CHANNEL_2};
// Motor_struct right_bottom_motor = {.htim = &htim1, .channel = TIM_CHANNEL_3};
void App_Flight_SetSpeed(Fly_status fly_status, Remote_struct *remote)
{

    // 根据飞行状态设置电机速度
    switch (fly_status)
    {
    case IDLE:
        // 飞行状态为空闲时，所有电机速度为0
        left_top_motor.speed = 0;
        left_bottom_motor.speed = 0;
        right_top_motor.speed = 0;
        right_bottom_motor.speed = 0;
        break;
    case NORMAL:
        // YAW => 变大 => 13 减速 24加速
        // PITCH => 变大 => 12 减速 34加速
        // ROLL => 变大 => 23减速 14加速

        left_top_motor.speed = remote->THR + pid_pitch_Gory.output +   pid_roll_Gory.output + LIMIT(pid_yaw_Gory.output, -100, 100);
        right_top_motor.speed = remote->THR + pid_pitch_Gory.output -  pid_roll_Gory.output - LIMIT(pid_yaw_Gory.output, -100, 100);

        right_bottom_motor.speed = remote->THR - pid_pitch_Gory.output -  pid_roll_Gory.output - LIMIT(pid_yaw_Gory.output, -100, 100);
        left_bottom_motor.speed = remote->THR - pid_pitch_Gory.output +  pid_roll_Gory.output + LIMIT(pid_yaw_Gory.output, -100, 100);
        // printf(":%d, %d, %d, %d", left_top_motor.speed, right_top_motor.speed, right_bottom_motor.speed, left_bottom_motor.speed);
        break;
    case FIX_HEIGHT:
        left_top_motor.speed = remote->THR + pid_pitch_Gory.output + pid_roll_Gory.output + LIMIT(pid_yaw_Gory.output, -100, 100) + LIMIT(pid_z_speed.output, -100, 100);
        right_top_motor.speed = remote->THR + pid_pitch_Gory.output - pid_roll_Gory.output - LIMIT(pid_yaw_Gory.output, -100, 100) + LIMIT(pid_z_speed.output, -100, 100);

        right_bottom_motor.speed = remote->THR - pid_pitch_Gory.output - pid_roll_Gory.output - LIMIT(pid_yaw_Gory.output, -100, 100) + LIMIT(pid_z_speed.output, -100, 100);
        left_bottom_motor.speed = remote->THR - pid_pitch_Gory.output + pid_roll_Gory.output + LIMIT(pid_yaw_Gory.output, -100, 100) + LIMIT(pid_z_speed.output, -100, 100);
        break;
    case FAULT:
        // 飞行状态为故障时，迫降,速度递减
        left_bottom_motor.speed -= 5;
        right_bottom_motor.speed -= 5;
        left_top_motor.speed -= 5;
        right_top_motor.speed -= 5;

        if ((left_bottom_motor.speed == 0) && (right_bottom_motor.speed == 0) && (left_top_motor.speed == 0) && (right_top_motor.speed == 0))
        {
            xTaskNotifyGive(Communication_Task_Handle);
        }

        break;
    default:
        // 其他情况，不做处理
        break;
    }
    // 限制电机速度在0到1000之间
    right_top_motor.speed = LIMIT(right_top_motor.speed, 0, 1500);
    right_bottom_motor.speed = LIMIT(right_bottom_motor.speed, 0, 1500);
    left_top_motor.speed = LIMIT(left_top_motor.speed, 0, 1500);
    left_bottom_motor.speed = LIMIT(left_bottom_motor.speed, 0, 1500);

    if (remote->THR < 100)
    {
        left_top_motor.speed = 0;
        left_bottom_motor.speed = 0;
        right_top_motor.speed = 0;
        right_bottom_motor.speed = 0;
    }

    // 设置电机速度
    Int_Motor_Setspeed(&left_top_motor);
    Int_Motor_Setspeed(&left_bottom_motor);
    Int_Motor_Setspeed(&right_top_motor);
    Int_Motor_Setspeed(&right_bottom_motor);
}

void App_Flight_Get_Euler_Angle(GyroAccel_Struct *gyro_accel, EulerAngle_Struct *eulerAngle)
{
    // 根据陀螺仪和加速度计数据计算欧拉角

    Int_MPU6050_Read_Gyro_Accel(gyro_accel);
    // 几率滤波前的值
    // 对角速度进行一节滤波
    gyro_accel->gyro.x = Com_Filter_LowPass(gyro_accel->gyro.x, gyro.x);
    gyro_accel->gyro.y = Com_Filter_LowPass(gyro_accel->gyro.y, gyro.y);
    gyro_accel->gyro.z = Com_Filter_LowPass(gyro_accel->gyro.z, gyro.z);
    gyro.x = gyro_accel->gyro.x;
    gyro.y = gyro_accel->gyro.y;
    gyro.z = gyro_accel->gyro.z;
    // 对加速度进行卡尔曼滤波
    gyro_accel->accel.x = Com_Filter_KalmanFilter(&kfs[0], gyro_accel->accel.x);
    gyro_accel->accel.y = Com_Filter_KalmanFilter(&kfs[1], gyro_accel->accel.y);
    gyro_accel->accel.z = Com_Filter_KalmanFilter(&kfs[2], gyro_accel->accel.z);

    // 调用Com_IMU_GetEulerAngle函数计算欧拉角
    Com_IMU_GetEulerAngle(gyro_accel, eulerAngle, 0.008);

    // COM_DEBUG_PRINTFln("pitch:%f", eulerAngle->pitch);
    //  COM_DEBUG_PRINTFln("gyro:%d,%d,%d", gyro_accel->gyro.x, gyro_accel->gyro.y, gyro_accel->gyro.z);
    // COM_DEBUG_PRINTFln("accel:%d,%d,%d", gyro_accel->accel.x, gyro_accel->accel.y, gyro_accel->accel.z);
}

void App_Flight_PIDControl(EulerAngle_Struct *eulerAngle, GyroAccel_Struct *gyro_accel, Remote_struct *remote)
{
    // 设定目标值
    pid_pitch.desire = (remote->PITCH - 1000) * 0.005;
    // 设定外环PID测量值
    pid_pitch.measure = eulerAngle->pitch;
    // 设置内环PID测量值
    pid_pitch_Gory.measure = (gyro_accel->gyro.y) * Gyro_G;

    Com_PID_CascadeCalculate(&pid_pitch, &pid_pitch_Gory, 0.008);

    pid_roll.desire = (remote->ROLL - 1000) * 0.005;
    // 设定外环PID测量值
    pid_roll.measure = eulerAngle->roll;
    // 设置内环PID测量值
    pid_roll_Gory.measure = (gyro_accel->gyro.x) * Gyro_G;

    Com_PID_CascadeCalculate(&pid_roll, &pid_roll_Gory, 0.008);

    pid_yaw.desire = (remote->YAW - 1000) * 0.005;
    // 设定外环PID测量值
    pid_yaw.measure = eulerAngle->yaw;
    // 设置内环PID测量值
    pid_yaw_Gory.measure = (gyro_accel->gyro.z) * Gyro_G;

    Com_PID_CascadeCalculate(&pid_yaw, &pid_yaw_Gory, 0.008);
}
// 记录上次的高度
float last_height = 0;
void App_Flight_Height_PIDControl(uint16_t height, uint16_t current_height, float dt)
{
    // 高度控制
    // 设定目标值
    pid_height.desire = height;
    // 设定外环PID测量值
    pid_height.measure = current_height;

    // 设置内环PID测量值
    pid_z_speed.measure = last_height + (((Com_IMU_GetNormAccZ() - 16384) / 16.384) * 9.8) * dt;
    // 记录上次的高度
    last_height = pid_z_speed.measure;

    Com_PID_CascadeCalculate(&pid_z_speed, &pid_height, dt);

    //COM_DEBUG_PRINTFln(" height:%d, current_height:%d, hight:%f, z_speed :%f,z_current_speed %f", height, current_height, pid_height.output, pid_z_speed.measure, pid_z_speed.output);
}
